
#ifndef PX_PARICLE_H
#define PX_PARICLE_H

#include <pxVector3.h>
#include <pxParticleIntegrator.h>

namespace Px {
namespace Physics {

template <typename Real>
class Particle {
    static_assert(std::is_integral<Real>::value || std::is_floating_point<Real>::value, "[Px:PointMass:type] Error: PointMass Real type must be an integral or floating point numerical representation.");

    using Vector3 = Math::Vector3<Real>;

public:
    Particle();
    Particle(const Particle& particle);
    Particle(const Vector3& position, Real mass = 1);
    Particle(const Vector3& position, const Vector3& velocity, Real mass = 1);
    virtual ~Particle();

    /* Update the position of the particle based on the internal integrator. */
    void update(Real dt);
    void reset();

    void setPosition(Real x, Real y, Real z);
    void setPosition(const Vector3& position);
    void addPosition(Real x, Real y, Real z);
    void addPosition(const Vector3& position);
    void setVelocity(Real vx, Real vy, Real vz);
    void setVelocity(const Vector3& velocity);
    void addVelocity(Real vx, Real vy, Real vz);
    void addVelocity(const Vector3& velocity);
    void setExternalForce(Real fx, Real fy, Real fz);
    void setExternalForce(const Vector3& force);
    void addExternalForce(Real fx, Real fy, Real fz);
    void addExternalForce(const Vector3& force);
    void setRestPosition(Real x, Real y, Real z);
    void setRestPosition(const Vector3& position);
    void setRestVelocity(Real x, Real y, Real z);
    void setRestVelocity(const Vector3& velocity);
    void setMass(Real mass);
    void setIntegrator(ParticleIntegrator<Real>* integrator);

    Vector3 getPreviousPosition() const;
    Vector3 getPosition() const;
    Vector3 getVelocity() const;
    Vector3 getExternalForce() const;
    Real getMass() const;

    Vector3& position();
    Vector3& velocity();
    Real& mass();

    const ParticleIntegrator<Real>* const getIntegrator();

    Particle<Real>& operator = (const Particle<Real>& particle);

    Real operator () (const Particle<Real>& particle) const;
    bool operator () (const Particle<Real>& a, const Particle<Real>& b) const;

protected:
    /*
     * Given the initial state problem, approximate the solution through a
     * given integration technique. The integrator is called within the update
     * function to modify the current velocity and position of the particle.
     *
     * POINTER_OWNERSHIP: NO
     */
    ParticleIntegrator<Real>* integrator;

    /* Mass of this particle. */
    Real m;

    /* 
     * Position and velocity of this particle. These are updated during the
     * course of the simulation (see: update()).
     */
    Vector3 prevPos, pos;
    Vector3 vel;
    Vector3 extForce;

    /* 
     * Rest position and velocity. These represent the state of the particle
     * at the beginning of the initial state problem that formulates the
     * physical simulation. These values are generally not changed during
     * the simulation.
     */
    Vector3 restPos;
    Vector3 restVel;
};

template <typename Real>
Particle<Real>::Particle() {
    this->m = static_cast<Real>(1);
    this->pos = Vector3::Zero();
    this->vel = Vector3::Zero();
    this->extForce = Vector3::Zero();
    this->restPos = Vector3::Zero();
    this->restVel = Vector3::Zero();

    this->integrator = nullptr;
}

template <typename Real>
Particle<Real>::Particle(const Particle& particle) {
    this->m = particle.m;
    this->pos = particle.pos;
    this->vel = particle.vel;
    this->extForce = particle.extForce;
    this->restPos = particle.restPos;
    this->restVel = particle.restVel;

    this->integrator = particle.integrator;
}

template <typename Real>
Particle<Real>::Particle(const Vector3& position, Real mass) {
    this->m = mass;
    this->pos = position;
    this->vel = Vector3::Zero();
    this->extForce = Vector3::Zero();
    this->restPos = position;
    this->restVel = Vector3::Zero();

    this->integrator = nullptr;
}

template <typename Real>
Particle<Real>::Particle(const Vector3& position, const Vector3& velocity, Real mass) {
    this->m = mass;
    this->pos = position;
    this->vel = velocity;
    this->extForce = Vector3::Zero();
    this->restPos = position;
    this->restVel = velocity;

    this->integrator = nullptr;
}

template <typename Real>
Particle<Real>::~Particle() {}

template <typename Real>
void Particle<Real>::update(Real dt) {
    if ( this->integrator == nullptr ) return;
    this->prevPos = this->pos;
    this->integrator->integrate(dt, this->m, this->pos, this->vel, this->extForce, this->pos, this->vel);
}

template <typename Real>
void Particle<Real>::reset() {
    this->pos = this->restPos;
    this->vel = this->restVel;
    this->prevPos = this->pos;
    this->extForce = Vector3::Zero();
}

template <typename Real>
void Particle<Real>::setPosition(Real x, Real y, Real z) {
    this->pos = Vector3(x, y, z);
}

template <typename Real>
void Particle<Real>::setPosition(const Vector3& position) {
    this->pos = position;
}

template <typename Real>
void Particle<Real>::addPosition(Real x, Real y, Real z) {
    this->pos.x() += x;
    this->pos.y() += y;
    this->pos.z() += z;
}

template <typename Real>
void Particle<Real>::addPosition(const Vector3& position) {
    this->pos += position;
}

template <typename Real>
void Particle<Real>::setVelocity(Real vx, Real vy, Real vz) {
    this->vel = Vector3(vx, vy, vz);
}

template <typename Real>
void Particle<Real>::setVelocity(const Vector3& velocity) {
    this->vel = velocity;
}

template <typename Real>
void Particle<Real>::addVelocity(Real vx, Real vy, Real vz) {
    this->vel.x() += vx;
    this->vel.y() += vy;
    this->vel.z() += vz;
}

template <typename Real>
void Particle<Real>::addVelocity(const Vector3& velocity) {
    this->vel += velocity;
}

template <typename Real>
void Particle<Real>::setExternalForce(Real fx, Real fy, Real fz) {
    this->extForce = Vector3(fx, fy, fz);
}

template <typename Real>
void Particle<Real>::setExternalForce(const Vector3& force) {
    this->extForce = force;
}

template <typename Real>
void Particle<Real>::addExternalForce(Real fx, Real fy, Real fz) {
    this->extForce.x() += fx;
    this->extForce.y() += fy;
    this->extForce.z() += fz;
}

template <typename Real>
void Particle<Real>::addExternalForce(const Vector3& force) {
    this->extForc += force;
}

template <typename Real>
void Particle<Real>::setRestPosition(Real x, Real y, Real z) {
    this->restPos = Vector3(x, y, z);
    this->prevPos = this->restPos;
}

template <typename Real>
void Particle<Real>::setRestPosition(const Vector3& position) {
    this->restPos = position;
    this->prevPos = this->restPos;
}

template <typename Real>
void Particle<Real>::setRestVelocity(Real vx, Real vy, Real vz) {
    this->restVel = Vector3(vx, vy, vz);
}

template <typename Real>
void Particle<Real>::setRestVelocity(const Vector3& velocity) {
    this->restVel = velocity;
}

template <typename Real>
void Particle<Real>::setMass(Real mass) {
    this->m = mass;
}

template <typename Real>
void Particle<Real>::setIntegrator(ParticleIntegrator<Real>* integrator) {
    if ( integrator == nullptr )
        std::cerr << "[Px:Particle:setIntegrator] Warning: Integrator has been set to a nullptr." << std::endl;
    this->integrator = integrator;
}

template <typename Real>
typename Particle<Real>::Vector3 Particle<Real>::getPreviousPosition() const {
    return this->prevPos;
}

template <typename Real>
typename Particle<Real>::Vector3 Particle<Real>::getPosition() const {
    return this->pos;
}

template <typename Real>
typename Particle<Real>::Vector3 Particle<Real>::getVelocity() const {
    return this->vel;
}

template <typename Real>
typename Particle<Real>::Vector3 Particle<Real>::getExternalForce() const {
    return this->extForce;
}

template <typename Real>
Real Particle<Real>::getMass() const {
    return this->m;
}

template <typename Real>
typename Particle<Real>::Vector3& Particle<Real>::position() {
    return this->pos;
}

template <typename Real>
typename Particle<Real>::Vector3& Particle<Real>::velocity() {
    return this->vel;
}

template <typename Real>
Real& Particle<Real>::mass() {
    return this->m;
}

template <typename Real>
const ParticleIntegrator<Real>* const Particle<Real>::getIntegrator() {
    return this->integrator;
}

template <typename Real>
Particle<Real>& Particle<Real>::operator = (const Particle<Real>& particle) {
    if ( this == &particle ) return *this;

    this->pos = particle.pos;
    this->vel = particle.vel;
    this->extForce = particle.extForce;
    this->integrator = particle.integrator;
    this->m = particle.m;
    this->prevPos = particle.prevPos;
    this->restPos = particle.restPos;
    this->restVel = particle.restVel;
    return *this;
}

template <typename Real>
Real Particle<Real>::operator () (const Particle<Real>& particle) const {
    return this->pos.x() + this->pos.y() + this->pos.z() + this->prevPos.x() + this->prevPos.y() + this->prevPos.z() + this->vel.x() + this->vel.y() + this->vel.z();
}

template <typename Real>
bool Particle<Real>::operator () (const Particle<Real>& a, const Particle<Real>& b) const {
    if ( a.pos == b.pos && a.prevPos == b.prevPos && a.vel = b.vel ) return true;
    return false;
}

typedef Particle<float> Particlef;
typedef Particle<double> Particled;

}

}

#endif
